#include "CGM.h"

using namespace Method;

CGM::CGM(shared_ptr<const Matrix> SLAEMatrix, shared_ptr<const Matrix> Preconditioner,
		shared_ptr<const Vector> RightPart, shared_ptr<const Vector> Approximation)
{
	_matrix = SLAEMatrix;
	_preconditioner = Preconditioner;
	_rightPart = RightPart;
	_approximation = Approximation;


	r1.resize(RightPart->size());
	r.resize(RightPart->size());
	MultAToP.resize(RightPart->size());
	z.resize(RightPart->size());
	_temp_vector.resize(RightPart->size());
	z1.resize(RightPart->size());
	_temp_vector.resize(RightPart->size());
	p.resize(RightPart->size());


	if (Approximation != nullptr)
		_result = shared_ptr<Vector>(new Vector(*Approximation));
	else _result = shared_ptr<Vector>(new Vector(_rightPart->size()));

	_matrix->MultMatrixVector(*_result, _temp_vector);
	r1 = *(const_cast<Vector *>(&(*_rightPart))) - _temp_vector;

	_preconditioner->multLInvToVector(true, r1, _temp_vector);
	_preconditioner->multUInvToVector(true, _temp_vector, z1);

	p = z1;

	_residual = 0;
}

CGM::~CGM()
{
}

void CGM::nextIteration() 
{
	_matrix->MultMatrixVector(p, MultAToP);
	alpha = r1 * z1 / (p * MultAToP);
	*_result += p * alpha;
	r = r1 - MultAToP * alpha;

	_matrix->multLInvToVector(true, r, _temp_vector);
	_matrix->multUInvToVector(true, _temp_vector, z);

	beta = z * r / (z1 * r1);
	p = z + p* beta;

	r1 = r;
	z1 = z;
	_residual = sqrt(r * r);
}
